﻿using System;
using System.Collections.Generic;
using System.Text;
using Allegro.Framework;
using Allegro.MathInterface;

namespace Allegro.Mathlib
{
    /// <summary>
    /// Cf. Numerical Recipes in C, ch. 10.2 and 10.3
    /// </summary>
    public class BrentFunctionMinimizationMethod : FunctionOptimization, IFunctionMinimization, IFunctionMinimizationWithDifferentiation
    {
        private const int ITMAX = 100;
        private const double CGOLD = 0.3819660;
        private const double ZEPS = 1.0e-10;

        public BrentFunctionMinimizationMethod()
        {
            Tolerance = 1.0e-6; // Default tolerance, should be reset to something appropriate
        }
        public BrentFunctionMinimizationMethod(double tolerance)
        {
            Tolerance = tolerance;
        }
        public double Tolerance { get; set; }

        #region Implementation of IFunctionMinimization

        /// <summary>
        /// Given a function f, and given a bracketing triplet of abscissas ax, x0 and cx, such that x0 is between
        /// ax and bx and f(x0) is less than both f(ax) and f(cx), this routing isolates the minimum to a fractional precision
        /// of about tol using Brent's method. The abscissa of the minimum is returned as xmin, and the minimum function value is returned
        /// as the return value from the function
        /// </summary>
        /// <param name="f"></param>
        /// <param name="ax"></param>
        /// <param name="cx"></param>
        /// <param name="x0"></param>
        /// <param name="xmin"></param>
        /// <returns></returns>
        public double FindMinimum(Function f, double ax, double cx, double x0, out double xmin)
        {
            int iter;
            double tol = Tolerance;
            double bx = x0;
            double a, b, d = 0.0, etemp, fu, fv, fw, fx, p, q, r, tol1, tol2, u, v, w, x, xm;
            double e = 0.0; // This will be the distance moved on the step before last

            // a and b must be in ascending order, but input ascissas need not be
            a = (ax < cx ? ax : cx);
            b = (ax > cx ? ax : cx);
            // Initializations
            x = w = v = bx;
            fw = fv = fx = f(x);

            for (iter = 0; iter < ITMAX; iter++) // Main loop
            {
                xm = 0.5 * (a + b);
                tol1 = tol * Math.Abs(x) + ZEPS;
                tol2 = 2.0 * tol1;
                if (Math.Abs(x - xm) <= (tol2 - 0.5 * (b - a)))
                {
                    // Test for done here
                    xmin = x;
                    return fx;
                }
                if (Math.Abs(e) > tol1)
                {
                    // Construct a trial parabolic fit
                    r = (x - w) * (fx - fv);
                    q = (x - v) * (fx - fw);
                    p = (x - v) * q - (x - w) * r;
                    q = 2.0 * (q - r);
                    if (q > 0.0)
                        p = -p;
                    q = Math.Abs(q);
                    etemp = e;
                    e = d;
                    if (Math.Abs(p) >= Math.Abs(0.5 * q * etemp) || p <= q * (a - x) || p >= q * (b - x))
                    {
                        // The above conditions determine the acceptability of the parabolic fit.
                        // Here we take the golden section step into the larger of the two segments
                        e = (x >= xm ? a - x : b - x);
                        d = CGOLD * e;
                    }
                    else
                    {
                        d = p / q; // Take the parabolic step
                        u = x + d;
                        if (u - a < tol2 || b - u < tol2)
                            d = Math.Sign(xm - x) * Math.Abs(tol1); // SIGN(tol1, xm-x)
                    }
                }
                else
                {
                    e = (x >= xm ? a - x : b - x);
                    d = CGOLD * e;
                }
                u = (Math.Abs(d) >= tol1 ? x + d : x + Math.Sign(d) * Math.Abs(tol1));
                fu = f(u); // This is the one function evaluation per iteration
                if (fu <= fx) // Now decide what to tdo with our function evaluation
                {
                    if (u >= x)
                        a = x;
                    else
                    {
                        b = x;
                    }
                    // Housekeeping follows:
                    _Shift(ref v, ref w, ref x, ref u);
                    _Shift(ref fv, ref fw, ref fx, ref fu);
                }
                else
                {
                    if (u < x)
                        a = u;
                    else
                    {
                        b = u;
                    }
                    if (fu <= fw || w == x)
                    {
                        v = w;
                        w = u;
                        fv = fw;
                        fw = fu;
                    }
                    else if (fu <= fv || v == x || v == w)
                    {
                        v = u;
                        fv = fu;
                    }
                    // Done with house keeping. Back for another iteration
                }
            }
            ErrorHandler.Error("BrentFunctionMinimizationMethod.FindMinimum: Too many iterations");
            xmin = x;
            return fx;
        }

        #endregion

        #region Implementation of IFunctionMinimizationWithDifferentiation

        /// <summary>
        /// Given a function f and its derivative function df, and given a bracketing triplet of abscissas ax, x0 and cx, such that x0 is between
        /// ax and bx and f(x0) is less than both f(ax) and f(cx), this routing isolates the minimum to a fractional precision
        /// of about tol using a modification of Brent's method that uses derivatives. The abscissa of the minimum is returned as xmin, 
        /// and the minimum function value is returned as the return value from the function.
        /// </summary>
        /// <param name="f"></param>
        /// <param name="df"></param>
        /// <param name="a"></param>
        /// <param name="cx"></param>
        /// <param name="x0"></param>
        /// <param name="xmin"></param>
        /// <returns></returns>
        public double FindMinimum(Function f, Function df, double ax, double cx, double x0, out double xmin)
        {
            int iter;
            double tol = Tolerance;
            double bx = x0;
            bool ok1, ok2;
            double a, b, d = 0.0, d1, d2, du, dv = 0.0, dw = 0.0, dx = 0.0, e = 0.0;
            double fu, fv, fw, fx, olde, tol1, tol2, u, u1, u2, v, w, x, xm;

            // a and b must be in ascending order, but input ascissas need not be
            a = (ax < cx ? ax : cx);
            b = (ax > cx ? ax : cx);
            // Initializations
            x = w = v = bx;
            fw = fv = fx = f(x);

            for (iter = 0; iter < ITMAX; iter++) // Main loop
            {
                xm = 0.5 * (a + b);
                tol1 = tol * Math.Abs(x) + ZEPS;
                tol2 = 2.0 * tol1;
                if (Math.Abs(x - xm) <= (tol2 - 0.5 * (b - a)))
                {
                    // Test for done here
                    xmin = x;
                    return fx;
                }
                if (Math.Abs(e) > tol1)
                {
                    d1 = 2.0 * (b - a);
                    d2 = d1;
                    if (dw != dx)
                        d1 = (w - x) * dx / (dx - dw); // Secant method with one point
                    if (dv != dx)
                        d2 = (v - x) * dx / (dx - dv); // And the other
                    // which of these two estimates of d shall we take? We will insist that they be within
                    // the bracket, and on the side pointed to by the derivative at x
                    u1 = x + d1;
                    u2 = x + d2;
                    ok1 = (a - u1) * (u1 - b) > 0.0 && dx * d1 <= 0.0;
                    ok2 = (a - u2) * (u2 - b) > 0.0 && dx * d2 <= 0.0;
                    olde = e; // Movement on the step before the last
                    e = d;
                    if (ok1 || ok2) // Take only an acceptable d, and if both are acceptable then take the smallest one
                    {
                        if (ok1 && ok2)
                            d = (Math.Abs(d1) < Math.Abs(d2) ? d1 : d2);
                        else if (ok1)
                            d = d1;
                        else
                        {
                            d = d2;
                        }
                        if (Math.Abs(d) <= Math.Abs(0.5 * olde))
                        {
                            u = x + d;
                            if (u - a < tol2 || b - u < tol2)
                                d = Math.Sign(xm - x) * Math.Abs(tol1); // SIGN(tol1, xm-x)
                        }
                        else
                        {
                            // Bisect, not golden section
                            e = (dx >= 0.0 ? a - x : b - x); // Decide which segment by the sign of the derivative
                            d = 0.5 * e;
                        }
                    }
                    else
                    {
                        e = (dx >= 0.0 ? a - x : b - x);
                        d = 0.5 * e;
                    }
                }
                else
                {
                    e = (dx >= 0.0 ? a - x : b - x);
                    d = 0.5 * e;
                }
                if (Math.Abs(d) >= tol1)
                {
                    u = x + d;
                    fu = f(u);
                }
                else
                {
                    u = x + Math.Sign(d) * Math.Abs(tol1);
                    fu = f(u);
                    if (fu > fx || d == 0.0) // d==0.0 is a spurious case, must take care of it (KJELL 02.05.2009)
                    {
                        // If the minimum step in the downhill direction takes us uphill, then we are done
                        xmin = x;
                        return fx;
                    }
                }
                // Now all the housekeeping
                du = df(u);
                if (fu <= fx)
                {
                    if (u >= x)
                        a = x;
                    else
                    {
                        b = x;
                    }
                    _Move(ref v, ref fv, ref dv, ref w, ref fw, ref dw);
                    _Move(ref w, ref fw, ref dw, ref x, ref fx, ref dx);
                    _Move(ref x, ref fx, ref dx, ref u, ref fu, ref du);
                }
                else
                {
                    if (u < x)
                        a = u;
                    else
                    {
                        b = u;
                    }
                    if (fu <= fw || w == x)
                    {
                        _Move(ref v, ref fv, ref dv, ref w, ref fw, ref dw);
                        _Move(ref w, ref fw, ref dw, ref u, ref fu, ref du);
                    }
                    else if (fu < fv || v == x || v == w)
                    {
                        _Move(ref v, ref fv, ref dv, ref u, ref fu, ref du);
                    }
                }
            }
            ErrorHandler.Error("BrentFunctionMinimizationMethod.FindMinimum: Too many iterations");
            xmin = x;
            return fx;
        }

        #endregion

    }
}
